/**********************************************
*  Namespace: AI
*  Classes:   Steering
*  Desc:      Updates positions using a steering
*             algorithm in 2d or 3d cases
*  Author:    Vaios Kalpias-Ilias (C) 2008-2009
***********************************************/

#include "Steering.h"
#include "Helper.h"
#include "EulerIntegrator.h"
#include "RK4Integrator.h"

namespace AI
{
	//---------------------------------------------
	/**
	* Ctor
	*/
	Steering::Steering()
	{
	}
	//---------------------------------------------
	/**
	* Dtor
	*/
	Steering::~Steering()
	{
	}
	//---------------------------------------------
	/**
	* Updates kinematic data in 2d
	*/
	void Steering::Update(Kinematic2d& kinematic, 
		                  Steering2d& steering,
						  float dt)
	{
		
		if (steering.linear.LengthFast() > maxAcceleration)
		{
			steering.linear = steering.linear.NormalizedFast() * maxAcceleration;
		}
		
		if (steering.angular > maxAngularAcceleration)
		{
			steering.angular = steering.angular *
				maxAngularAcceleration/ abs(steering.angular);
		}
		
		Phx::EulerIntegrator::Integrate<mth::Vector2>(kinematic.position,
									    kinematic.velocity,
										steering.linear,
										dt);
		
		if (kinematic.velocity.LengthFast() > maxSpeed)
		{
			kinematic.velocity.NormalizeFast();
			kinematic.velocity *= maxSpeed;
		}
		
		if (kinematic.velocity.LengthFast() > 0.0f)
		{
			kinematic.velocity -= velocityDamping * kinematic.velocity.NormalizedFast(); 
		}

		Phx::RK4Integrator::Integrate<float>(kinematic.orientation,
									    kinematic.rotation,
										steering.angular,
										dt);

		
		kinematic.orientation = 
			mth::Helper::
			ClampAngle(kinematic.orientation, -mth::M_PI, mth::M_PI);

		float rotationSize = abs(kinematic.rotation);
		if (rotationSize > maxRotation)
		{
			kinematic.rotation = kinematic.rotation / rotationSize;
			kinematic.rotation *= maxRotation;
		}

		if (rotationSize > 0.0f)
		{
			kinematic.rotation -= kinematic.rotation * 
				rotationDamping/ rotationSize;
			
		}
		
		
	}
}
